#include "zf_driver_file.h"
#include "zf_common_headfile.h"
#include "Fuzz_pid.h"
#include "PID.h"
//获取外部定义
extern int Mid_Angle;
//定义的PID
PIDtypedef Moter_Left;
PIDtypedef Moter_Right;
PIDtypedef Servo;
//350 0.30
//400 0.25
float KP2=0;
float P_Base=0.25;
// float GKND=-0.00035;//变速650
float GKND=-0.00000;
//初始化PID（准备转向模糊PID）
void PID_Init()
{
    //默认电机参数
    Moter_Right.Kp=7.0;
    Moter_Right.Ki=1.3;
    Moter_Right.Kd=0.3;
    //默认舵机参数实际使用模糊pid
    Servo.Kp=0.0;
    // Servo.Ki=0.10;//650
    // Servo.Ki=0.0;
    if(Speed_Target==650)
    {
        Servo.Ki=0.10;
    }
    else
    {
        Servo.Ki=0.0;
    }
    //350
    // Servo.Kd=0.93;
    //400 0.88
    //400 0.93
    // 450 0.77
    // Servo.Kd=0.75;//500
    // Servo.Kd=0.8;//550 1
    // Servo.Kd=1.15;1.1
    // Servo.Kd=0.90;//变速650
    // Servo.Kd=0.80;//new 变速650
    // Servo.Kd=0.75;//变速650
    //固定d
    // if(Speed_Target==650)
    // {
    //     Servo.Kd=0.75;
    // }
    // else
    // {
    //     Servo.Kd=0.75;
    // }
    Servo.Kd=0;
}
//设置电机参数
void Get_Motor_PID()
{
    if(Speed_Target>=250 && Speed_Target<300)
    {
        Moter_Right.Kp=8.49;
        Moter_Right.Ki=0.7;
        Moter_Right.Kd=0.3;
    }
    else if(Speed_Target>=300 && Speed_Target<350)
    {
        Moter_Right.Kp=5.78;
        Moter_Right.Ki=0.9;
        Moter_Right.Kd=0.3;
    }
    else if(Speed_Target>=350 && Speed_Target<400)
    {
        Moter_Right.Kp=6.10;
        Moter_Right.Ki=0.8;
        Moter_Right.Kd=0.3;
    }
    else if(Speed_Target>=400 && Speed_Target<450)
    {
        Moter_Right.Kp=9.50;
        Moter_Right.Ki=1.2;
        Moter_Right.Kd=0.3;
    }
    else if(Speed_Target>=450 && Speed_Target<500)
    {
        Moter_Right.Kp=10.50;
        Moter_Right.Ki=1.2;
        Moter_Right.Kd=0.3;
    }
    else if(Speed_Target>=500 && Speed_Target<550)
    {
        Moter_Right.Kp=14.0;
        Moter_Right.Ki=1.6;
        Moter_Right.Kd=0.3;
    }
    else if(Speed_Target>=550 && Speed_Target <600)
    {
        Moter_Right.Kp=16.0;
        Moter_Right.Ki=1.7;
        Moter_Right.Kd=0.3;
    }
    else if(Speed_Target>=600 && Speed_Target <650)
    {
        Moter_Right.Kp=19.0;
        Moter_Right.Ki=1.9;
        Moter_Right.Kd=0.3;
    }
    else if(Speed_Target>=650 && Speed_Target <700)
    {
        Moter_Right.Kp=13.0;
        Moter_Right.Ki=2.6;
        Moter_Right.Kd=0.0;
    }
    else if(Speed_Target>=700 && Speed_Target <750)
    {
        Moter_Right.Kp=16.0;
        Moter_Right.Ki=8.9;
        Moter_Right.Kd=0.0;
    }
    else if(Speed_Target>=750 && Speed_Target <800)
    {
        Moter_Right.Kp=16.0;
        Moter_Right.Ki=9.9;
        Moter_Right.Kd=0.0;
    }
}
//左电机PID
int Moter_Left_PID(int target, int actual) {
    Moter_Left.err = (target - actual);
    // 计算PID增量
    Moter_Left.PWM_OUT += Moter_Left.Kp * (Moter_Left.err - Moter_Left.err_last)
                        + Moter_Left.Ki * Moter_Left.err
                        + Moter_Left.Kd * (Moter_Left.err - 2*Moter_Left.err_last + Moter_Left.err_last_last);
    // 限幅处理
    if (Moter_Left.PWM_OUT > 5000) {
        Moter_Left.PWM_OUT = 5000;
    } else if (Moter_Left.PWM_OUT < -5000) {
        Moter_Left.PWM_OUT = -5000;
    }
    // 更新历史误差
    Moter_Left.err_last_last = Moter_Left.err_last;
    Moter_Left.err_last = Moter_Left.err;
    return Moter_Left.PWM_OUT;
}
//右电机PID
int Moter_Right_PID(int target, int actual) {
    Moter_Right.err = (target - actual);
    // 计算PID增量
    Moter_Right.PWM_OUT += Moter_Right.Kp * (Moter_Right.err - Moter_Right.err_last)
                         + Moter_Right.Ki * Moter_Right.err
                         + Moter_Right.Kd * (Moter_Right.err - 2*Moter_Right.err_last + Moter_Right.err_last_last);
    // 限幅处理
    if (Moter_Right.PWM_OUT > 6000) {
        Moter_Right.PWM_OUT = 6000;
    } else if (Moter_Right.PWM_OUT < -6000) {
        Moter_Right.PWM_OUT = -6000;
    }
    // 更新历史误差
    Moter_Right.err_last_last = Moter_Right.err_last;
    Moter_Right.err_last = Moter_Right.err;
    return Moter_Right.PWM_OUT;
}
//舵机PD控制
float Servo_PID(float servo_err ,int Mud_err)
{ 
    Servo.err=servo_err-Mud_err;
    //模糊p  350 0.0012
    // 400 0.0029
    // Servo.Kp=P_Base+abs(servo_err)*0.0029;
    Servo.Kp=Fuzzy_P(servo_err,(Servo.err-Servo.err_last));
    Servo.Angle=Servo.Kp*Servo.err+Servo.Kd*(Servo.err-Servo.err_last)+Servo.Ki*(Servo.err - 2*Servo.err_last + Servo.err_last_last)+imu660ra_gyro_z*GKND+79.4;
    Servo.err_last=Servo.err;
    if(Servo.Angle>92.4)
    {
        Servo.Angle=92.4;
    }
    else if(Servo.Angle<66.4)
    {
        Servo.Angle=66.4;
    }
    return Servo.Angle;
}
//模糊pid
float filtered_gyro = 0;
float Fuzz_PID(float servo_err ,int Mud_err)
{
    //计算误差
    Servo.err=servo_err-Mud_err;
    //模糊p
    Servo.Kp=Fuzzy(servo_err,(Servo.err-Servo.err_last));
    //避障固定p防止抖动
    if(BiZhang_Flag==2 || BiZhang_Flag==1)
    {
        if(Speed_Target>=550)
        {
            Servo.Kp=0.11;
        }
        else
        {
            Servo.Kp=0.15;
        }
    }
    //在特殊元素中给定p值
     if(HuanDao_Stas==3 || HuanDao_Stas==4)
    {
        if(Speed_Target==550)
        {
            Servo.Kp=0.37;
        }
        else if(Speed_Target==600)
        {
            Servo.Kp=0.40;
        }
        else
        {
            Servo.Kp=0.3;            
        }
    }
    else if( HuanDao_Stas==10 || HuanDao_Stas==11)
    {
        if(Speed_Target==550)
        {
            Servo.Kp=0.37;
        }
        else if(Speed_Target==600)
        {
            Servo.Kp=0.40;
        }
        else
        {
            Servo.Kp=0.3;            
        }
    }
    if(Speed_Target>550)
    {
        // GKND=-0.00035;
    }
    else
    {
        GKND=0;
    }
    //按不同速度值给定d
    if(Speed_Target==350)
    {
        Servo.Kd=0.450;
    }
    if(Speed_Target==400)
    {
        Servo.Kd=0.65;
    }
    else if(Speed_Target==450)
    {
        Servo.Kd=0.65;
    }
    else if(Speed_Target==500)
    {
        Servo.Kd=0.68;
    }
    else if(Speed_Target==550)
    {
        Servo.Kd=0.70;
    }
    else if(Speed_Target==600)
    {
        Servo.Kd=0.70;
    }
    filtered_gyro = 0.8f * filtered_gyro + 0.2f * imu660ra_gyro_z;
    //计算角度
    Servo.Angle=Servo.Kp*Servo.err+Servo.Kd*(Servo.err-Servo.err_last)+filtered_gyro*GKND+80.4;
    //数据迭代
    Servo.err_last=Servo.err;
    //输出限幅
    if(Servo.Angle>92.4)
    {
        Servo.Angle=92.4;
    }
    else if(Servo.Angle<68.4)
    {
        Servo.Angle=68.4;
    }
    return Servo.Angle;
}

// 快速正切函数（可用标准tan替代）
// float FastTan(float rad) {
//     return tanf(rad); // 实际项目中可替换为查表或近似计算
// }

// void AKM_Analysis(float velocity , int32 angle)
// {
//     //C车模数据
// 	float Car_L_ = 205; //长(mm)
//     float Car_T_ = 153; //宽(mm)
//     float K_Turn_;
//     float Rad;
//     angle = LIMIT(angle, -30, 30);
//     Rad = DEGTORAD(angle);
//     K_Turn_ = std::tan(Rad) * Car_T_/2/Car_L_;
//     K_Turn_ = LIMIT(K_Turn_,-5,5);
//     Left_Speed = velocity * (1+K_Turn_);
//     Right_Speed = velocity * (1-K_Turn_);
// }
